﻿#include "CalaDist_Tool.h"
#include <QCoreApplication>
#include <stdio.h>
#include <QString>
/******************************************************************************
  @Project Name  : 03TestFCLCollision
  @File Name     : main.cpp
  @Version       : 1.0
  @Function      : to be determined
  @Author        : unishuai
  @Date          : 2024-05-20
******************************************************************************/
#include <chrono>
#include <thread>
int main()
{
    std::array<double,5> angle={0,0,0,0,0};
    CalcDist_Tool::Instance();
    QTime myTime;
    int elapsedTime ;
    /**=================by unishuai====================
    *1.检测连杆机构和飞机的碰撞(仅仅只是碰撞)
    *===================2024-05-29 ====================*/
//    myTime.start();
//    std::vector<DistMsg> ret=CalcDist_Tool::Instance().checkArmWithSlider2FlightWithAngleJustCol(0,0,angle,0,{27.887,0,0});
//    elapsedTime = myTime.elapsed();  // 获取从start到现在的经过时间（毫秒）
//    std::cout << "Function run time: " << elapsedTime << " ms" << std::endl;
//    for(int i =0;i<5;i++){
//        std::cout<<ret[i]<<std::endl;
//    }

    /**=================by unishuai====================
    *2.动平台和洞壁的碰撞检测(仅仅只是碰撞)
    *===================2024-05-29 ====================*/
//        myTime.start();
//    //    std::array<double,6> platformPose={1609.25,
//    //                                       -160.64,
//    //                                       263.11,
//    //                                       -10.93,
//    //                                       4.72,
//    //                                       14.36
//    //                                      };
//            std::array<double,6> platformPose={1114.72,
//                                               53.22,
//                                               130.56,
//                                               336.33,
//                                               106.01,
//                                               -15.81
//                                              };
//            std::array<double,1> missileAngle={0};
//            std::vector<DistMsg> ret2=CalcDist_Tool::Instance().checkMotionPlatform2TunnelWallJustCol(platformPose,missileAngle);
//        elapsedTime = myTime.elapsed();  // 获取从start到现在的经过时间（毫秒）
//        std::cout << "Function run time: " << elapsedTime << " ms" << std::endl;
//        for(int i =0;i<ret2.size();i++){
//            std::cout<<ret2[i]<<std::endl;
//        }

    /**=================by unishuai====================
    *3.动平台和飞机的碰撞(仅仅只是碰撞)
    *===================2024-05-20 ====================*/
////            std::array<double,6> platformPose={1600.00,
////                                               -70.00,
////                                               248.00,
////                                               0,
////                                               0,
////                                               0
////                                              };
////            std::array<double,1> missileAngle={0};
////            double flightAngle=0;


//        //    std::array<double,6> platformPose={1471.26,
//        //                                       -149.97,
//        //                                       256.17,
//        //                                       322.55,
//        //                                       31.25,
//        //                                       21.41
//        //                                      };
//        //    std::array<double,1> missileAngle={180};
//        //    double flightAngle=-166.6;


//    std::array<double,6> platformPose={1604.09,
//                                       -89.75,
//                                       255.37,
//                                       2.59,
//                                       0.87,
//                                       3.96
//                                      };
//    std::array<double,1> missileAngle={0};
//    double flightAngle=0;
//    myTime.start();
//    std::vector<DistMsg> ret3=CalcDist_Tool::Instance().checkMotionPlatformWithPose2FlightWithAngleJustCol(0,platformPose,missileAngle,0,flightAngle);
//    elapsedTime = myTime.elapsed();  // 获取从start到现在的经过时间（毫秒）
//    std::cout << "Function run time: " << elapsedTime << " ms" << std::endl;
//    for(int j = 0; j < ret3.size(); j++)
//    {
//        std::cout<<ret3[j] <<"\n";
//    }

    /**=================by unishuai====================
    *4.检测平台中，一边杆的碰撞检测(仅仅只是碰撞)
    *===================2024-05-20 ====================*/
//        myTime.start();
//        std::vector<std::vector<double>> anglesSide1={
//            std::vector<double>{9,-9.3981,-0.119771,-19.0347,23.7593},
//            std::vector<double>{9,-11.2554,-1.77518,-159.516,-5.70736},
//            std::vector<double>{18,-6.70611,-2.24347,-15.1849,-21.0674},
//        };

//        int sideNum=0;
//        std::vector<std::vector<DistMsg>> infoData= CalcDist_Tool::Instance().checkArmWithSliderBySideNumJustCol(sideNum,anglesSide1);
//        for(int i=0;i<infoData.size();i++){
//            for(int j=0;j<infoData[i].size();j++){
//                std::cout<<infoData[i][j]<<std::endl;;
//            }
//        }

//        ///另一边
//        std::vector<std::vector<double>> anglesSide2={
//            std::vector<double>{0,4.48384,1.54305,-1.99904,-18.8451},
//            std::vector<double>{0,4.79466,0.87311,-2.10666,-19.156},
//            std::vector<double>{0,5.1343,-0.0236149,0.179662,-29.4106},
//        };
//        int sideNum2=1;
//        std::vector<std::vector<DistMsg>> infoData2= CalcDist_Tool::Instance().checkArmWithSliderBySideNumJustCol(sideNum2,anglesSide2);
//        for(int i=0;i<infoData2.size();i++){
//            for(int j=0;j<infoData2[i].size();j++){
//                std::cout<<infoData2[i][j]<<std::endl;;
//            }
//        }
//        elapsedTime = myTime.elapsed();  // 获取从start到现在的经过时间（毫秒）
//        std::cout << "Function run time: " << elapsedTime << " ms" << std::endl;



    //-------------------------------------------------------------------------------------------------------------------
    /**=================by unishuai====================
    *1.连杆机构和飞机的碰撞
    *===================2024-05-20 ====================*/
    myTime.start();
    std::vector<DistMsg> ret1=CalcDist_Tool::Instance().checkArmWithSlider2FlightWithAngle(4,0,angle,0,{43.099,0,0});
        for(int j = 0; j < 5; j++)
        {
            std::cout<<ret1[j] <<"\n";
        }
         elapsedTime = myTime.elapsed();
        std::cout << "2222Function run time: " << elapsedTime << " ms" << std::endl;

    /**=================by unishuai====================
    *2.动平台和洞壁的碰撞检测
    *===================2024-05-20 ====================*/
////        std::array<double,6> platformPose={1609.25,
////                                           -160.64,
////                                           263.11,
////                                           -10.93,
////                                           4.72,
////                                           14.36
////                                          };
//        std::array<double,6> platformPose={1600,
//                                           -69.998,
//                                           247.998,
//                                           0,
//                                           0,
//                                           0
//                                          };
//        std::array<double,1> missileAngle={0};
//        std::vector<DistMsg> ret=CalcDist_Tool::Instance().checkMotionPlatform2TunnelWall(platformPose,missileAngle);
//        for(int j = 0; j < ret.size(); j++)
//        {
//            std::cout<<ret[j] <<"\n";
//        }

    /**=================by unishuai====================
    *3.动平台和飞机的碰撞
    *===================2024-05-20 ====================*/
//        std::array<double,6> platformPose={1600.00,
//                                           -69.998,
//                                           247.998,
//                                           0,
//                                           0,
//                                           0
//                                          };
//        std::array<double,1> missileAngle={0};
//        std::array<double,3> flightAngle={0,0,0};


//    //    std::array<double,6> platformPose={1471.26,
//    //                                       -149.97,
//    //                                       256.17,
//    //                                       322.55,
//    //                                       31.25,
//    //                                       21.41
//    //                                      };
////        std::array<double,1> missileAngle={180};
////        double flightAngle=-166.6;

//        std::vector<DistMsg> ret=CalcDist_Tool::Instance().checkMotionPlatformWithPose2FlightWithAngle(0,platformPose,missileAngle,0,flightAngle);
//        for(int j = 0; j < ret.size(); j++)
//        {
//            std::cout<<ret[j] <<"\n";
//        }

    /**=================by unishuai====================
    *4.检测平台中，一边杆的碰撞检测
    *===================2024-05-20 ====================*/
//        std::vector<std::vector<double>> anglesSide1={
//            std::vector<double>{9,-9.3981,-0.119771,-19.0347,23.7593},
//            std::vector<double>{9,-11.2554,-1.77518,-159.516,-5.70736},
//            std::vector<double>{18,-6.70611,-2.24347,-15.1849,-21.0674},
//        };

//        int sideNum=0;
//        std::vector<std::vector<DistMsg>> infoData= CalcDist_Tool::Instance().checkArmWithSliderBySideNum(sideNum,anglesSide1);
//        for(int i=0;i<infoData.size();i++){
//            for(int j=0;j<infoData[i].size();j++){
//                std::cout<<infoData[i][j]<<std::endl;;
//            }
//        }

//        ///另一边
//        std::vector<std::vector<double>> anglesSide2={
//            std::vector<double>{0,4.48384,1.54305,-1.99904,-18.8451},
//            std::vector<double>{0,4.79466,0.87311,-2.10666,-19.156},
//            std::vector<double>{0,5.1343,-0.0236149,0.179662,-29.4106},
//        };
//        int sideNum2=1;
//        std::vector<std::vector<DistMsg>> infoData2= CalcDist_Tool::Instance().checkArmWithSliderBySideNum(sideNum2,anglesSide2);
//        for(int i=0;i<infoData2.size();i++){
//            for(int j=0;j<infoData2[i].size();j++){
//                std::cout<<infoData2[i][j]<<std::endl;;
//            }
//        }














    //-----------------------------------------------------------------------------------------------------------------

    /**=================by unishuai====================
    *测试
    * checkArmWithSlider2FlightWithAngle
    *===================2024-05-07 ====================*/
    //    std::array<double,6> platformPose={1609.25,
    //                                       -160.64,
    //                                       263.11,
    //                                       -10.93,
    //                                       4.72,
    //                                       14.36
    //                                      };
    //    std::array<std::array<double,5>,6> angleTotal={
    //        std::array<double, 5>{9,-9.3981,-0.119771,-19.0347,23.7593},
    //        std::array<double, 5>{9,-11.2554,-1.77518,-159.516,-5.70736},
    //        std::array<double, 5>{18,-6.70611,-2.24347,-15.1849,-21.0674},
    //        std::array<double, 5>{0,4.48384,1.54305,-1.99904,-18.8451},
    //        std::array<double, 5>{0,4.79466,0.87311,-2.10666,-19.156},
    //        std::array<double, 5>{0,5.1343,-0.0236149,0.179662,-29.4106},


    //    };

    //    std::array<double,6> platformPose={1600.0,
    //                                       -70.0,
    //                                       248.00,
    //                                       0.0,
    //                                       0.0,
    //                                       0.0
    //                                      };

    //    std::array<double,5> angle={0,0,0,0,0};

    //    std::vector<DistMsg> ret=CalcDist_Tool::Instance().checkArmWithSlider2FlightWithAngle(0,0,angle,0,22.5);
    //    std::cout<<"机械臂"<<0<<"与动平台的距离："<<"\n";
    //    for(int j = 0; j < 5; j++)
    //    {
    //        std::cout<<ret[j] <<"\n";
    //    }

    /**=================by unishuai====================
    *测试
    * checkMotionPlatform2StaticPlatform
    *===================2024-05-07 ====================*/
    //        std::cout<<CalcDist_Tool::Instance().checkMotionPlatform2StaticPlatform(platformPose);

    /**=================by unishuai====================
    *测试
    * checkArmWithSlider2FlightWithAngle
    *===================2024-05-07 ====================*/
    //        std::cout<<CalcDist_Tool::Instance().checkPlatFormWithPose2FlightWithAngle(0,platformPose,0,0.8)[0];













    /**=================by unishuai====================
    *下面的内容全部丢弃了
    *===================2024-05-07 ====================*/
    //    CalcDist_Tool tool;
    //    tool.initMod("./ce6.xml");

    //    std::array<double,6> angle={0,0,0,0,0,0};
    //    std::array<double,6> ret=tool.checkArm2Cabin(0,0,angle,0);
    //    std::array<double,6> ret2=tool.checkArm2SW(0,0,angle,1,45);

    //    Eigen::Matrix4d T_in=tool.RobotObjs[0].m_TransIiNow[6]*RobotBasic::PE2T(1000,1000,1000,0,90,0);
    //    std::array<double,7> ret3=tool.checkCombinedArm2Cabin(2,T_in,angle,0);

    //    Eigen::VectorXd rangle(4);
    //    rangle<<0,0,0,0;
    //    std::vector<DistMsg> ret4=tool.checkArm2Radar(0,3,angle,0,rangle);

    //    int i = 0;
    //    std::cout<<"机械臂与着陆器距离：" <<"\n";
    //    for (i = 0; i < 6; i++)
    //    {
    //        std::cout<<ret[i] <<"\n";
    //    }
    //    std::cout<<"机械臂与太阳翼距离："<<"\n";
    //    for (i = 0; i < 6; i++)
    //    {
    //        std::cout<<ret2[i] <<"\n";
    //    }

    //    tool.initMod("./ce6change.xml");



    //ur10的碰撞检测
    //    tool.initMod("./ur10.xml");
    //    std::array<double,6> angle={0,0,180,0,0};
    //    for(int i=1;i<7;i++){
    //        std::array<double,6> ret=tool.checkArm2Self(0,0,angle,i);
    //        std::cout<<"机械臂与自身第"<<i<<"模块的距离："<<"\n";
    //        for(int j = 0; j < 6; j++)
    //        {
    //            std::cout<<ret[j] <<"\n";
    //        }
    //    }


    //    tool.initMod("./jetColObjModel/jetCol.xml");
    /**=================by unishuai====================
    *滑块和自身的距离
    *===================2024-05-04 ====================*/
    //    std::array<double,5> angle={0,0,0,0,0};
    //    for(int i=0;i<5;i++){
    //        std::array<double,5> ret=tool.checkArm2Self(0,0,angle,i);
    //        std::cout<<"机械臂与自身第"<<i<<"模块的距离："<<"\n";
    //        for(int j = 0; j < 5; j++)
    //        {
    //            std::cout<<ret[j] <<"\n";
    //        }

    //    }

    /**=================by unishuai====================
    *滑块机械臂和飞机的距离
    *===================2024-05-04 ====================*/
    //    std::array<double,5> angle={0,0,0,0,0};
    //    for(int i=0;i<6;i++){
    //        std::array<double,5> ret=tool.checkArm2Flight(i,0,angle,0);
    //       std::cout<<"机械臂"<<i<<"与飞机的距离："<<"\n";
    //        for(int j = 0; j < 5; j++)
    //        {
    //            std::cout<<ret[j] <<"\n";
    //        }
    //    }

    //        std::array<double,5> angle={0,0,0,0,0};
    //        for(int i=0;i<6;i++){
    //            std::array<double,5> ret=tool.checkArm2Platform(i,0,angle,1);
    //            std::cout<<"机械臂"<<i<<"与静平台的距离："<<"\n";
    //            for(int j = 0; j < 5; j++)
    //            {
    //                std::cout<<ret[j] <<"\n";
    //            }
    //        }


    /**=================by unishuai====================
    *动平台
    *===================2024-05-04 ====================*/
    //        std::array<double,5> angle={0,0,0,0,0};
    //        std::array<double,6> platformPose={1600,-70,248,0,0,0};
    //        for(int i=0;i<6;i++){
    //            std::array<double,5> ret=tool.checkArm2PlatformWithPose(i,0,angle,0,platformPose);
    //            std::cout<<"机械臂"<<i<<"与动平台的距离："<<"\n";
    //            for(int j = 0; j < 5; j++)
    //            {
    //                std::cout<<ret[j] <<"\n";
    //            }
    //        }

    /**=================by unishuai====================
    *测试一下新写的通过滑动控制碰撞的代码
    *===================2024-05-04 ====================*/
    //    std::array<std::array<double,5>,6> angleTotal={
    //        std::array<double, 5>{9,-9.3981,-0.119771,-19.0347,23.7593},
    //        std::array<double, 5>{9,-11.2554,-1.77518,-159.516,-5.70736},
    //        std::array<double, 5>{18,-6.70611,-2.24347,-15.1849,-21.0674},
    //        std::array<double, 5>{0,4.48384,1.54305,-1.99904,-18.8451},
    //        std::array<double, 5>{0,4.79466,0.87311,-2.10666,-19.156},
    //        std::array<double, 5>{0,5.1343,-0.0236149,0.179662,-29.4106},


    //    };
    //    std::array<double,6> platformPose={1609.25,
    //                                       -160.64,
    //                                       263.11,
    //                                       -10.93,
    //                                       4.72,
    //                                       14.36};

    //    for(int i=0;i<6;i++){
    //        std::array<double,5> ret=tool.checkArmWithSlider2PlatformWithPose(i,0,angleTotal[i],0,platformPose);
    //        std::cout<<"机械臂"<<i<<"与动平台的距离："<<"\n";
    //        for(int j = 0; j < 5; j++)
    //        {
    //            std::cout<<ret[j] <<"\n";
    //        }
    //    }

    /**=================by unishuai====================
    *测试一下飞机那个是不是可以使用的
    *===================2024-05-04 ====================*/


    //        std::array<double,5> angle={0,0,0,0,0};
    //        std::array<double,6> platformPose={1606.84,-91.44,243.25,1.00,-2.57,5.20};
    //        std::array<double,1> ret=tool.checkPlatFormWithPose2FlightWithAngle(0,platformPose,0,0);
    //        std::cout<<"动平台和"<<"与飞机的距离为："<<ret[0]<<"\n";

    //        std::array<double,5> angle={3,0,0,0,0};
    //        for(int i=0;i<6;i++){
    //           std::array<double,5> ret=tool.checkArmWithSlider2FlightWithAngle(i,0,angle,0,21);
    //           std::cout<<"机械臂"<<i<<"与飞机的距离："<<"\n";
    //            for(int j = 0; j < 5; j++)
    //            {
    //                std::cout<<ret[j] <<"\n";
    //            }
    //        }

    /**=================by unishuai====================
    *测试一下静平台和动平台的碰撞检测
    *===================2024-05-06 ====================*/
    //        std::array<double,6> platformPose={1749.26,-288.90,329.15,204.74,-187.41,6.77};
    //    double ret=tool.checkMotionPlatform2StaticPlatform(platformPose);
    //    std::cout<<"动平台"<<" "<<"与静平台的距离："<<ret<<"\n";







    return 0;
}
